#pragma once

#ifndef NDEBUG
#include <iostream>
#endif

#include <Ogre.h>

#include "Utility.h"



struct QTreeNodeData
{
	float error;
	float minSqrDistance;
};



template<typename T>
struct QuadTreeNode
{
	typedef Ogre::Vector2 Pos2D;
		
	QuadTreeNode(void) : mCenter(Pos2D(0.0f, 0.0f))
	{
	}
	QuadTreeNode(const QuadTreeNode<T>& iOther) : mData(iOther.mData), mCenter(iOther.mCenter)
	{
	}
	QuadTreeNode<T>& operator=(const QuadTreeNode<T>& iOther)
	{
		//QuadTreeNode<T> copy(iOther);
		//copy.Swap(*this);
		mData = iOther.mData;
		mCenter = iOther.mCenter;
		return *this;
	}

	T mData;
	Pos2D mCenter;
};



template<typename T>
class QuadTree
{
public:

	typedef Ogre::Vector2 Pos2D;
	typedef std::vector<QuadTreeNode<T>> NodesVector;

	static const size_t FAST_NODES_COUNT_MASK = 0x55555555;

	QuadTree(const size_t iWorldResolution, const size_t iMinCellResolution, Pos2D& iCenter);

	size_t GetWorldResolution(void) const;
	size_t GetMinCellResolution(void) const;
	size_t GetMaxNumLevels(void) const;

	const QuadTreeNode<T>& GetParent(const size_t iNodeId) const;
	const QuadTreeNode<T>& GetNorthOvestChild(const size_t iNodeId) const;
	const QuadTreeNode<T>& GetNorthEastChild(const size_t iNodeId) const;
	const QuadTreeNode<T>& GetSouthOvestChild(const size_t iNodeId) const;
	const QuadTreeNode<T>& GetSouthEastChild(const size_t iNodeId) const;

	const QuadTreeNode<T>& GetNode(const size_t iNodeId) const;
	QuadTreeNode<T>& GetNode(const size_t iNodeId);

	bool IsLastLevel(const size_t iLevel) const;

	template<typename FunctionObject>
	void PreOrderTraversal(FunctionObject iTransformFunc);

	template<typename FunctionObject>
	void PostOrderTraversal(FunctionObject iTransformFunc);

private:

	void BuildQuadTree(const size_t iNodeResolution, const Pos2D& iNodeCenter, const size_t iNodeId);

	size_t GetParentId(const size_t iNodeId) const;
	size_t GetNorthOvestChildId(const size_t iNodeId) const;
	size_t GetNorthEastChildId(const size_t iNodeId) const;
	size_t GetSouthOvestChildId(const size_t iNodeId) const;
	size_t GetSouthEastChildId(const size_t iNodeId) const;

	size_t QuadTree<T>::GetLevelNodesCount(const size_t iLevel) const;

	template<typename FunctionObject>
	void PreOrderStep(FunctionObject iTransformFunc, const size_t iNodeId, const size_t iLevel);

	template<typename FunctionObject>
	void PostOrderStep(FunctionObject iTransformFunc, const size_t iNodeId, const size_t iLevel);

	NodesVector mNodesVector;

	size_t mWorldResolution;
	size_t mMinCellResolution;
	size_t mMaxNumLevels;
};




template<typename T>
QuadTree<T>::QuadTree(const size_t iWorldResolution, const size_t iMinCellResolution, Pos2D& iCenter) :
	mWorldResolution(iWorldResolution),
	mMinCellResolution(iMinCellResolution),
	mMaxNumLevels(0)
{
	mMaxNumLevels = FindMostSignificantBitSetIndex(iWorldResolution/iMinCellResolution) +1;
	mNodesVector.resize(GetLevelNodesCount(mMaxNumLevels-1));

	BuildQuadTree(iWorldResolution, iCenter, 0);
}



template<typename T>
size_t QuadTree<T>::GetWorldResolution(void) const
{
	return mWorldResolution;
}



template<typename T>
size_t QuadTree<T>::GetMinCellResolution(void) const
{
	return mMinCellResolution;
}



template<typename T>
size_t QuadTree<T>::GetMaxNumLevels(void) const
{
	return mMaxNumLevels;
}



template<typename T>
size_t QuadTree<T>::GetParentId(const size_t iNodeId) const
{
	return (iNodeId-1) >> 2;
}



template<typename T>
size_t QuadTree<T>::GetNorthOvestChildId(const size_t iNodeId) const
{
	return (iNodeId << 2) + 1;
}



template<typename T>
size_t QuadTree<T>::GetNorthEastChildId(const size_t iNodeId) const
{
	return (iNodeId << 2) + 2;
}



template<typename T>
size_t QuadTree<T>::GetSouthEastChildId(size_t iNodeId) const
{
	return (iNodeId << 2) + 3;
}



template<typename T>
size_t QuadTree<T>::GetSouthOvestChildId(const size_t iNodeId) const
{
	return (iNodeId << 2) + 4;
}



template<typename T>
const QuadTreeNode<T>& QuadTree<T>::GetNode(const size_t iNodeId) const
{
	assert(iNodeId < mNodesVector.size());
	return mNodesVector[iNodeId];
}



template<typename T>
QuadTreeNode<T>& QuadTree<T>::GetNode(const size_t iNodeId)
{
	assert(iNodeId < mNodesVector.size());
	return mNodesVector[iNodeId];
}



template<typename T>
const QuadTreeNode<T>& QuadTree<T>::GetParent(const size_t iNodeId) const
{
	assert(iNodeId < mNodesVector.size());
	return mNodesVector[GetParentId(iNodeId)];
}



template<typename T>
const QuadTreeNode<T>& QuadTree<T>::GetNorthOvestChild(const size_t iNodeId) const
{
	assert(iNodeId < mNodesVector.size());
	return mNodesVector[GetNorthOvestChildId(iNodeId)];
}



template<typename T>
const QuadTreeNode<T>& QuadTree<T>::GetNorthEastChild(const size_t iNodeId) const
{
	assert(iNodeId < mNodesVector.size());
	return mNodesVector[GetNorthEastChildId(iNodeId)];
}



template<typename T>
const QuadTreeNode<T>& QuadTree<T>::GetSouthOvestChild(const size_t iNodeId) const
{
	assert(iNodeId < mNodesVector.size());
	return mNodesVector[GetSouthOvestChildId(iNodeId)];
}



template<typename T>
const QuadTreeNode<T>& QuadTree<T>::GetSouthEastChild(const size_t iNodeId) const
{
	assert(iNodeId < mNodesVector.size());
	return mNodesVector[GetSouthEastChildId(iNodeId)];
}



template<typename T>
size_t QuadTree<T>::GetLevelNodesCount(const size_t iLevel) const
{
	// Returns:
	//   4^0 + 4^1 + ... + 4^iLevel =
	// = 2^0*2^0 + 2^1*2^1 + ... + 2^iLevel*2^iLevel =
	// = 2^0 + 2^2 + ... + 2^(iLevel*2)

	assert(iLevel < mMaxNumLevels);
	size_t maxExpNum = 0x01 << (iLevel*2);
	return (maxExpNum|(maxExpNum-1)) & FAST_NODES_COUNT_MASK;
}



template<typename T>
bool QuadTree<T>::IsLastLevel(const size_t iLevel) const
{
	assert(iLevel < mMaxNumLevels);
	return (iLevel == (mMaxNumLevels-1));
}



template<typename T>
void QuadTree<T>::BuildQuadTree(const size_t iNodeResolution, const Pos2D& iNodeCenter, const size_t iNodeId)
{
	assert(iNodeId < mNodesVector.size());

	const size_t semiResolution = iNodeResolution/2;
	const size_t quarterResolution = semiResolution/2;

	mNodesVector[iNodeId].mCenter = iNodeCenter;

	if(mMinCellResolution < iNodeResolution)
	{
		BuildQuadTree(semiResolution, Pos2D(iNodeCenter.x - quarterResolution, iNodeCenter.y + quarterResolution),
			GetNorthOvestChildId(iNodeId));
		BuildQuadTree(semiResolution, Pos2D(iNodeCenter.x + quarterResolution, iNodeCenter.y + quarterResolution),
			GetNorthEastChildId(iNodeId));
		BuildQuadTree(semiResolution, Pos2D(iNodeCenter.x + quarterResolution, iNodeCenter.y - quarterResolution),
			GetSouthEastChildId(iNodeId));
		BuildQuadTree(semiResolution, Pos2D(iNodeCenter.x - quarterResolution, iNodeCenter.y - quarterResolution),
			GetSouthOvestChildId(iNodeId));
	}
}



template<typename T> template<typename FunctionObject>
void QuadTree<T>::PreOrderTraversal(FunctionObject iTransformFunc)
{
	assert(mNodesVector.size() > 0);
		PreOrderStep(iTransformFunc, 0, 0);
}



template<typename T> template<typename FunctionObject>
void QuadTree<T>::PostOrderTraversal(FunctionObject iTransformFunc)
{
	assert(mNodesVector.size() > 0);
		PostOrderStep(iTransformFunc, 0, 0);
}



template<typename T> template<typename FunctionObject>
void QuadTree<T>::PreOrderStep(FunctionObject iTransformFunc, const size_t iNodeId, const size_t iLevel)
{
	assert(iNodeId < mNodesVector.size());
	bool keepGoing = iTransformFunc(*this, iNodeId, iLevel);
	if(keepGoing)
	{
		const size_t nextLevel = iLevel+1;
		if(nextLevel < mMaxNumLevels)
		{
			PreOrderStep(iTransformFunc, GetSouthOvestChildId(iNodeId), nextLevel);
			PreOrderStep(iTransformFunc, GetSouthEastChildId(iNodeId), nextLevel);
			PreOrderStep(iTransformFunc, GetNorthOvestChildId(iNodeId), nextLevel);
			PreOrderStep(iTransformFunc, GetNorthEastChildId(iNodeId), nextLevel);
		}
	}
}



template<typename T> template<typename FunctionObject>
void QuadTree<T>::PostOrderStep(FunctionObject iTransformFunc, const size_t iNodeId, const size_t iLevel)
{
	assert(iNodeId < mNodesVector.size());

	const size_t nextLevel = iLevel+1;
	if(nextLevel < mMaxNumLevels)
	{
		PostOrderStep(iTransformFunc, GetSouthOvestChildId(iNodeId), nextLevel);
		PostOrderStep(iTransformFunc, GetSouthEastChildId(iNodeId), nextLevel);
		PostOrderStep(iTransformFunc, GetNorthOvestChildId(iNodeId), nextLevel);
		PostOrderStep(iTransformFunc, GetNorthEastChildId(iNodeId), nextLevel);
	}
	iTransformFunc(*this, iNodeId, iLevel);
}